/***************************************************************
 * Name:      proGSOF.cpp
 * Purpose:   Defines Application Frame
 * Author:    walnutcy (walnutcy@gmail.com)
 * Created:   2012-09-14
 * Copyright: walnutcy (weibo.com/walnutcy)
 * License:
 **************************************************************/
#include "proGSOF.h"
#include "wxLogTrace.h"
#include <wx/stattext.h>


proGSOF::proGSOF()
{
    //ctor
    deinit();
}

proGSOF::~proGSOF()
{
    //dtor
}

void proGSOF::deinit(void)
{
    pMsg = &msg;
    pMsg->state = GSOF_HEAD;
}

u8  proGSOF::protocol(u8 ch)
{
    u8  rst = 0;

    if (NULL == pMsg)	return rst;

    if (pMsg->state == GSOF_HEAD)
    {
        if ((0x02 == pMsg->mDat[1])
                &&(0x40 == ch))
        {
            pMsg->state   = GSOF_A_LEN;
            pMsg->rlen = 0;
            pMsg->status = pMsg->mDat[2];
        }
        else
        {
            pMsg->mDat[0] = pMsg->mDat[1];
            pMsg->mDat[1] = pMsg->mDat[2];
            pMsg->mDat[2] = ch;
        }
    }
    else if (pMsg->state == GSOF_PACKET)
    {
        pMsg->mDat[pMsg->rlen] = ch;
        pMsg->rlen++;
        if (pMsg->rlen >= pMsg->plen)/* a check */
        {
            pMsg->state = GSOF_CHECK;
        }
        else if (pMsg->rlen > (GSOF_PACKET_MAX - 2)) // overflow
        {
            pMsg->state = GSOF_HEAD;
        }
    }
    else if (pMsg->state == GSOF_A_LEN)
    {
        pMsg->plen = ch;
        pMsg->rlen = 0;
        pMsg->state = GSOF_PACKET;
    }
    else if (pMsg->state == GSOF_CHECK)
    {
        pMsg->check = ch;
        pMsg->state = GSOF_ETX;
    }
    else if (pMsg->state == GSOF_ETX)
    {
        if (0x03==ch)
            rst = 1;
        pMsg->state = GSOF_HEAD;
    }
    else
    {
        pMsg->state   = GSOF_HEAD;
    }

    return rst;
}

f4  proGSOF::libBIN2F4(u8* pDat)
{
    UN_FLOAT res;

    res.bDat[3] = pDat[0];
    res.bDat[2] = pDat[1];
    res.bDat[1] = pDat[2];
    res.bDat[0] = pDat[3];

    return res.fData;
}

f8  proGSOF::libBIN2F8(u8* pDat)
{
    UN_DOUBLE res;

    res.bDat[7] = pDat[0];
    res.bDat[6] = pDat[1];
    res.bDat[5] = pDat[2];
    res.bDat[4] = pDat[3];

    res.bDat[3] = pDat[7];
    res.bDat[2] = pDat[6];
    res.bDat[1] = pDat[5];
    res.bDat[0] = pDat[4];

    return res.dData;
}

#define libBIN2_U16(var,p)  {var = p[0]; var <<= 8; var += p[1];}
#define libBIN2_U32(var,p)  {var = p[0]; var <<= 8; var += p[1];var <<= 8; var += p[2];var <<= 8; var += p[3];}
#define pi   3.1415926

void  proGSOF::packGSOF_08(void* pParam, u8* p)
{
    f4 t;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    plog->log(_("%02X, "), p[2]);
    t = libBIN2F4(&p[3]);
    plog->log(_("%9.3f, "), t);
    t = libBIN2F4(&p[7]);
    plog->log(_("%7.3f,"), t);
    t = libBIN2F4(&p[11]);
    plog->log(_("%10.3f"), t);
    plog->log(_("\r\n"));

}

void  proGSOF::packGSOF_14(void* pParam, u8* p)
{
    u8  i;
    s8  elev;
    u16 azim;
    f4  snr1;
    f4  snr2;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    plog->log(_("PRN num: %02d\r\n"), p[2]);
    plog->log(_("    prn,f1,f2,elev,azim,snr1,snr2\r\n"));

    for(i=0; i<p[2]; i++)
    {
        elev = (s8)(p[6+i*8]);
        azim = p[7+i*8];
        azim <<= 8;
        azim += p[8+i*8];
        snr1  = (f4)(p[9+i*8]);
        snr2  = (f4)(p[10+i*8]);
        snr1 /= 4;
        snr2 /= 4;
        plog->log(_("    PRN: %02d %02X %02X %02d %03d %5.2f %5.2f\r\n"),
                  p[3+i*8], p[4+i*8], p[5+i*8], elev, azim, snr1, snr2);
    }
}

void  proGSOF::packGSOF_34(void* pParam, u8* p)
{
    u8  i;
    s8  elev;
    u16 azim;
    f4  snr1;
    f4  snr2;
    wxChar  tsys[10];
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    plog->log(_("PRN num: %02d\r\n"), p[2]);
    plog->log(_("    prn,f1,f2,elev,azim,snr1,snr2\r\n"));

    for(i=0; i<p[2]; i++)
    {
        elev = (s8)(p[7+i*10]);
        azim = p[8+i*10];
        azim <<= 8;
        azim += p[9+i*10];
        snr1  = (f4)(p[10+i*10]);
        snr2  = (f4)(p[11+i*10]);
        snr1 /= 4;
        snr2 /= 4;
        if (0 == p[4+i*10])
        {
            wxStrcpy(tsys, _("GP")); // gps
        }
        else if (1 == p[4+i*10])
        {
            wxStrcpy(tsys, _("SB")); // sbas
        }
        else if (2 == p[4+i*10])
        {
            wxStrcpy(tsys, _("GL")); // glonass
        }
        else if (10 == p[4+i*10])
        {
            wxStrcpy(tsys, _("OM")); // omnistar
        }
        else
        {
            wxStrcpy(tsys, _("RE")); // reserved
        }
        plog->log(_("    PRN: %s%03d %02X %02X %02d %03d %5.2f %5.2f\r\n"),
                  tsys, p[3+i*10], p[5+i*10], p[6+i*10], elev, azim, snr1, snr2);
    }
}

void  proGSOF::packGSOF_09(void* pParam, u8* p)
{
    f4 t;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    t = libBIN2F4(&p[2]);
    plog->log(_("PDOP: %5.2f, "), t);
    t = libBIN2F4(&p[6]);
    plog->log(_("HDOP: %5.2f\r\n"), t);

}

void  proGSOF::packGSOF_02(void* pParam, u8* p)
{
    f8 t;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    t = libBIN2F8(&p[2]);
    t *= 180.0/pi;
    plog->log(_("LLA84: %11.6lf, "), t);
    t = libBIN2F8(&p[10]);
    t *= 180.0/pi;
    plog->log(_("%11.6lf, "), t);
    t = libBIN2F8(&p[18]);
    plog->log(_("%9.3lf, "), t);
    plog->log(_("\r\n"));

}

void  proGSOF::packGSOF_01(void* pParam, u8* p)
{
    u8* sp;
    u16 week;
    u32 tow;
    f4  tt;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    sp = &p[2];
    libBIN2_U32(tow, sp);
    tt = tow/1000.0;
    sp = &p[6];
    libBIN2_U16(week, sp);
    plog->log(_("week: %4d, tow: %10.3f\r\n"), week, tt);

}

void  proGSOF::packGSOF_27(void* pParam, u8* p)
{
    u8* sp;
    f8  t8;
    u32 tow;
    wxLogTrace* plog = (wxLogTrace*)pParam;

    if (plog == NULL) return;
    if (NULL == p)  return;

    sp = &p[2];
    libBIN2_U32(tow, sp);
    plog->log(_("%10d,"), tow);

    t8 = libBIN2F8(&p[10]);
    plog->log(_(" %6.2lf, "), t8);
    t8 = libBIN2F8(&p[18]);
    plog->log(_(" %6.2lf, "), t8);
    t8 = libBIN2F8(&p[26]);
    plog->log(_(" %6.2lf, "), t8);
    t8 = libBIN2F8(&p[34]);
    plog->log(_("range: %9.3lf, "), t8);

    plog->log(_("\r\n"));

}

void  proGSOF::decPacket(void* pParam)
{
    u8* p;
    u16  len;
    u8  txNo;
    wxLogTrace* plog = (wxLogTrace*)pParam;
    if (plog == NULL) return;

    p = &pMsg->mDat[3];
    len = pMsg->plen-3; // txno,pageindex,max_page
    txNo = pMsg->mDat[0];

    //if (pMsg->mDat[1]) return; // no support 2012.07.17
    if (pMsg->mDat[2])
    {
        if ((pMsg->mDat[1] == 0)&&(pMsg->mDat[2]))
        {
            pMsg->alen = len-3;
            memcpy(pMsg->aDat, p, len-3);
            return;
        }
        else if (pMsg->mDat[1] < pMsg->mDat[2])
        {
            if ((pMsg->alen+len-3) < GSOF_DATA_MAX)
            {
                memcpy(&pMsg->aDat[pMsg->alen], p, len-3);
                pMsg->alen += len-3;
            }
            else
            {
                plog->log(_("overflow, drop: %d\r\n"), len);
            }
            return;
        }
        else if (pMsg->mDat[1] == pMsg->mDat[2])
        {
            if ((pMsg->alen+len-3) < GSOF_DATA_MAX)
            {
                memcpy(&pMsg->aDat[pMsg->alen], p, len-3);
                pMsg->alen += len-3;
            }
            else
            {
                plog->log(_("overflow, drop: %d\r\n"), len);
            }

            p = &pMsg->aDat[0];
            len = pMsg->alen;
        }
    }

    do
    {
        plog->log(_("%3d<%02d,%3d>:"), txNo, p[0], p[1]);
        if (len < (2+p[1]))
            break;

        if (0x08==p[0]) // GSOF 08, velocity data
        {
            packGSOF_08(pParam, p);
        }
        else if (1==p[0]) // position time
        {
            packGSOF_01(pParam, p);
        }
        else if (2==p[0]) // lat,lon,alt
        {
            packGSOF_02(pParam, p);
        }
        else if (9==p[0]) // pdop
        {
            packGSOF_09(pParam, p);
        }
        else if (34==p[0]) // sv detailed info
        {
            packGSOF_34(pParam, p);
        }
        else if (27==p[0]) // attitude info
        {
            packGSOF_27(pParam, p);
        }
        else if (14==p[0]) // sv detailed info
        {
            //packGSOF_14(pParam, p); // GSOF 34 include all sv info
            plog->log(_("\r\n"));
        }
        else
        {
            plog->log(_("\r\n"));
        }
        if (len <= (2+p[1]))
            break;
        len -= (2+p[1]);
        p += 2+p[1];
    }
    while(1);

    return;
}
